#include "widget.h"
#include <QApplication>
#include <QThread>
#include <QObject>
#include <QShortcut>
#include <QEventLoop>
#include <QtConcurrent/QtConcurrent>
#include "rosconnector.h"
#include "trajPlanning.h"
#include <QVector>
#include "vsPhotometric.h"
#include <unistd.h>

TrajPlanning traj_planning;
RosConnector* connector = nullptr;

//关节规划子函数
bool trajectoryJointPlan(QEventLoop* loop)
{  
    vector<vector<double>> motors_now = {{621.13,509.75,0,0,0,-pi/2},
                                        {536.65,575.08,0,0,0,-pi/2},
                                        {536.65,575.08,0,0,0,-pi/2},
                                        {594.66,571.27,0,0,0,-pi/2},
                                        {601.13,529.75,0,0,0,-pi/2},
                                        {621.13,509.75,0,0,0,-pi/2},
                                        {536.65,575.08,0,0,0,-pi/2},
                                        {536.65,575.08,0,0,0,-pi/2},
                                        {594.66,571.27,0,0,0,-pi/2},
                                        {601.13,529.75,0,0,0,-pi/2}};//change for array
    vector<double> motors_start(11);
    vector<double> motors_start_dj(4);
    connector->kine.Param_writeto_Inv_sol(motors_now[connector->i_operate]);
    
    // if (connector->i_operate == 1 || connector->i_operate == 2 || connector->i_operate == 6 || connector->i_operate == 7){
    //     motors_start=connector->kine.Inv_sol_addh(connector->i_operate);
    // }else{
        motors_start=connector->kine.Inv_sol(connector->i_operate, connector->i_screw );
    // }

    //if ((connector->i_operate == 3 && connector->i_screw == 2) || connector->i_operate == 8){
    //    motors_start[10] = -28;
    //}
    //

    //需要读出每个电机的初始角，赋给q_array
    vector<vector<double>> q_array={{ connector->motors[0],motors_start[0]} , { connector->motors[1],motors_start[1] }, { connector->motors[2],motors_start[2] }, 
    { connector->motors[3],motors_start[3] }, { connector->motors[4],motors_start[4] },  { connector->motors[5],motors_start[5]}, { connector->motors[10],motors_start[10]}};
    
    vector<double> time={0, 9};
    
    vector<vector<double>> q_traj = traj_planning.jointSpacePlan(q_array, time);
    for (int i=0; i<q_traj.size(); i++){
        for(int j=0;j<6;++j){
            connector->motors[j]=q_traj[i][j];
        }
        motors_start_dj = connector->kine.joint2motorAngle(connector->motors[0],connector->motors[1],connector->motors[2]);
        for(int j=0;j<4;++j){
            connector->motors[j+6]=motors_start_dj[j];
        }
        connector->motors[10]=q_traj[i][6];
        std::cout<<"关节角:   "<< connector->motors[0]<<" "<< connector->motors[1]<<" "<< connector->motors[2]<<" "<< connector->motors[3]<<" "<< connector->motors[4]<<" "<< connector->motors[5]<<" "<< connector->motors[10]<<std::endl;
        //std::cout<<"丝驱动电机： "<< connector->motors[6]<<" "<<connector->motors[7]<<" "<<connector->motors[8]<<" "<<connector->motors[9]<<" "<<endl;
        connector->pub_all_robot(connector->motors);

        connector->kine.Param_joints_update_simulation(connector->motors);
        connector->adjustmotor(connector->motors);
        // std::vector<double> traj_start_position1;
        // traj_start_position1.resize(9);
    
        // if (connector->i_operate == 1 || connector->i_operate == 2 || connector->i_operate == 6 || connector->i_operate == 7){
        // traj_start_position1  = connector->kine.kec_joints2End_addh(connector->i_operate);
        //  }else{
        // traj_start_position1  = connector->kine.kec_joints2End();
        // }
        // std::cout<<"末端位置"<<traj_start_position1[0]<<"   "<<traj_start_position1[1]<<"   "<<traj_start_position1[2]<<"   "<<traj_start_position1[3]<<"   "<<traj_start_position1[4]<<"   "<<traj_start_position1[5]<<endl;
 


        // connector->pub_total_position(connector->motors,connector->i_operate); //仿真中控制关节速度

        i = i + 1;
    }
    loop->quit();
    return true;
}

//探视关节规划子函数TrajPlanning::catesianSpacePlanOnLine
bool trajectoryJointPlan_notic(QEventLoop* loop, bool secondrun, bool joint_back)
{   
    vector<vector<double>> motors_dest = {{1.27,0.24,PI/2,0,1.31,0,0,0,0,0,-8.17},
                                        {1.31,0.21,PI/2-0.35,1.48,2.09,0,0,0,0,0,-8.17},
                                        {1.41,0.175,PI/2 - 0.52,1.05,2.11,0,0,0,0,0,-80},
                                        {1.41,0.175,PI/2 - 0.52,1.05,2.11,0,0,0,0,0,-80},
                                        {1.41,0.175,PI/2 + 0.52,-1.05,2.11,0,0,0,0,0,-80},
                                        {1.31,0.21,PI/2+0.35,-1.48,2.09,0,0,0,0,0,-8.17},
                                        {1.27,0.24,PI/2,0,1.31,0,0,0,0,0,-8.17},
                                        {1.57,0,PI/2,0,PI/2,0,0,0,0,0,0}};//change for array
    vector<double> motors_dest_dj(4);

    vector<vector<double>> q_array;

    // //600位置两段规划
    // if((connector->i_coverPlate == 1 || connector->i_coverPlate == 5) && !secondrun && !joint_back){
    //     motors_dest[connector->i_coverPlate][3]=0;
    // }

    if(joint_back){
        //需要读出每个电机的初始角，赋给q_array
        q_array={{ connector->motors[0],motors_dest[7][0]} , { connector->motors[1],motors_dest[7][1] }, { connector->motors[2],motors_dest[7][2] }, 
        { connector->motors[3],motors_dest[7][3] }, { connector->motors[4],motors_dest[7][4] },  { connector->motors[5],motors_dest[7][5]}, { connector->motors[10],motors_dest[7][10]}};   
    }else{
        //需要读出每个电机的初始角，赋给q_array
        q_array={{ connector->motors[0],motors_dest[connector->i_coverPlate][0]} , { connector->motors[1],motors_dest[connector->i_coverPlate][1] }, { connector->motors[2],motors_dest[connector->i_coverPlate][2] }, 
        { connector->motors[3],motors_dest[connector->i_coverPlate][3] }, { connector->motors[4],motors_dest[connector->i_coverPlate][4] },  { connector->motors[5],motors_dest[connector->i_coverPlate][5]}, { connector->motors[10],motors_dest[connector->i_coverPlate][10]}};
    }
     
    vector<double> time={0, 9};
    
    vector<vector<double>> q_traj = traj_planning.jointSpacePlan(q_array, time);
    for (int i=0; i<q_traj.size(); i++){
        for(int j=0;j<6;++j){
            connector->motors[j]=q_traj[i][j];
        }
        motors_dest_dj = connector->kine.joint2motorAngle(connector->motors[0],connector->motors[1],connector->motors[2]);
        for(int j=0;j<4;++j){
            connector->motors[j+6]=motors_dest_dj[j];
        }
        connector->motors[10]=q_traj[i][6];

        // std::cout<<"关节角:   "<< connector->motors[0]<<" "<< connector->motors[1]<<" "<< - connector->motors[2]<<" "<< connector->motors[3]<<" "<< connector->motors[4]<<" "<< connector->motors[5]<<" "<< connector->motors[10]<<std::endl;
        connector->pub_all_robot(connector->motors);
        // connector->pub_total_position(connector->motors,connector->i_operate); //仿真中控制关节速度
        i = i + 2;
    }
    loop->quit();
    return true;
}

//笛卡尔轨迹规划子函数
bool screwPosControl(QEventLoop* loop)
{
    

    connector->kine.Param_joints_update_simulation(connector->motors);
    std::vector<double> traj_start_position;
    traj_start_position.resize(9);
    
    if (connector->i_operate == 1 || connector->i_operate == 2 || connector->i_operate == 6 || connector->i_operate == 7){
        traj_start_position  = connector->kine.kec_joints2End_addh(connector->i_operate);
    }else{
        traj_start_position  = connector->kine.kec_joints2End();
    }
    
    for(int i=0; i<3; i++){
        connector->cartesian_position[connector->i_operate][connector->i_screw][0][i] = traj_start_position[i];
        connector->cartesian_orientation[connector->i_operate][connector->i_screw][0][i] = traj_start_position[i+3];
        std::cout<<connector->cartesian_position[connector->i_operate][connector->i_screw][0][i]<<"   "<<connector->cartesian_orientation[connector->i_operate][connector->i_screw][0][i]<<endl;
    }
        connector->kine.Param_writeto_Inv_sol(traj_start_position);

        // if (connector->i_operate == 1 || connector->i_operate == 2 || connector->i_operate == 6 || connector->i_operate == 7)
        // {
        //     connector->motors=connector->kine.Inv_sol_addh(connector->i_operate);
        // }
        // else
        // {
            connector->motors=connector->kine.Inv_sol(connector->i_operate, connector->i_screw );
        // }
        std::cout<<connector->motors[0]<<" "<<connector->motors[1]<<" "<<connector->motors[2] <<" "<<connector->motors[3]<<" "<<connector->motors[4]<<" "<<connector->motors[5]<<" "<<connector->motors[10] <<endl;


    vector<vector<double>> target_cartesian = traj_planning.catesianSpacePlan(connector->cartesian_position[connector->i_operate][connector->i_screw],connector->cartesian_orientation[connector->i_operate][connector->i_screw]);
    
    // Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> kinetempCar;
    // Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> temp;
    // // std::vector<vector<double>> kinetempCar;
    // // kinetempCar.resize(target_cartesian.size(),11);

    // // std::vector<vector<double>> temp;
    // // temp.resize(target_cartesian.size(),11);

    for (int i=0; i<target_cartesian.size(); i++){
        
        connector->kine.Param_writeto_Inv_sol(target_cartesian[i]);
         
         std::cout<<endl;

         std::cout<<"位置: "<<target_cartesian[i][0]<<" "<<target_cartesian[i][1]<<" "<<target_cartesian[i][2]<<"姿态: "<<target_cartesian[i][3]<<" "<<target_cartesian[i][4]<<" "<<target_cartesian[i][5]<<" "<<endl;
    
        // if (connector->i_operate == 1 || connector->i_operate == 2 || connector->i_operate == 6 || connector->i_operate == 7)
        // {
        //     connector->motors=connector->kine.Inv_sol_addh(connector->i_operate);
        // }
        // else if(connector->i_operate == 0 || connector->i_operate == 5)

        if(connector->i_operate == 0 || connector->i_operate == 5)
        {
            
            std::vector<double>  kinetemp=connector->kine.Inv_sol(connector->i_operate, connector->i_screw );

            connector->motors[0]=kinetemp[0];
            connector->motors[1]=kinetemp[1];
            connector->motors[2]=kinetemp[2];
            connector->motors[6]=kinetemp[6];
            connector->motors[7]=kinetemp[7];
            connector->motors[8]=kinetemp[8];
            connector->motors[9]=kinetemp[9];
            connector->motors[10]=kinetemp[10];

            // connector->motors=connector->kine.Inv_sol(connector->i_operate, connector->i_screw );

            connector->kine.Param_joints_update_simulation_for(kinetemp);

            std::cout<< "关节角度  " <<connector->motors[0]<<" "<<connector->motors[1]<<" "<<connector->motors[2] <<" "<< kinetemp[3]<<" "<< kinetemp[4]<<" "<< kinetemp[5]<<" "<<connector->motors[10] <<endl;

        }
        else
        {
            connector->motors=connector->kine.Inv_sol(connector->i_operate, connector->i_screw );


            connector->kine.Param_joints_update_simulation_for(connector->motors);
        }
    
        
        connector->pub_all_robot(connector->motors);
        connector->adjustmotor(connector->motors);
        // connector->pub_total_position(connector->motors,connector->i_operate);
        i = i + 2;
    }
    loop->quit();
    return true;
}

bool screwVisualServo(QEventLoop* loop)
{

    std::vector<double> pointxy = {263.146240234375, 89.83887481689453, 243.709716796875, 92.89017486572266, 246.978759765625, 112.07119750976562, 266.4444580078125, 109.2481460571289,};

    std::vector<cv::Point2f> des_Four_Centers;
    std::vector<double> des_apriltag_positions;
    des_apriltag_positions.resize(3);
    des_apriltag_positions[0]= -0.00018671069870095547;              ////期望位置
    des_apriltag_positions[1]=-0.03140824833168978;
    des_apriltag_positions[2]= 0.08985104128005852;
    

    des_Four_Centers.resize(4);

    des_Four_Centers[0]=cv::Point2f(pointxy[0], pointxy[1]);
    des_Four_Centers[1]=cv::Point2f(pointxy[2], pointxy[3]);
    des_Four_Centers[2]=cv::Point2f(pointxy[4], pointxy[5]);
    des_Four_Centers[3]=cv::Point2f(pointxy[6], pointxy[7]);

    double focallength_pix=(836.211243+841.879883)/2;
    double u0=296.402466,v0=226.237866;

    std::cout<<"-----------------------FLAG vslaw声明----------------------"<<endl;
    // std::cout<<"des_Four_Centers: "<<des_Four_Centers<<endl;
    VS_4Point vslaw(des_Four_Centers,focallength_pix,u0,v0, des_apriltag_positions);

    std::cout<<"-----------------------FLAG 开始控制----------------------"<<endl;
    vslaw.VScontrol_simulation();
    loop->quit();

    if(vslaw.VS_success){


        return true;

    }else{
        return false;
    }
}

//手动遥操作子函数
bool manualModeExample(QEventLoop* loop)
{
    connector->manualPattern = true;
    connector->button_3_check = false;

    // 添加手动位置初始
    connector->kine.Param_joints_update_simulation(connector->motors);   
    if (connector->i_operate == 1 || connector->i_operate == 2 || connector->i_operate == 6 || connector->i_operate == 7){
        connector->set_manual_start(connector->kine.kec_joints2End_addh(connector->i_operate));
    }else{
        connector->set_manual_start(connector->kine.kec_joints2End());
    }
    while(connector->manualPattern){
        ros::spinOnce();
    }
    loop->quit();
    return true;
}

//末端轴线下落子函数
bool motionControl_Down(QEventLoop* loop)
{
    connector->kine.Param_joints_update_simulation(connector->motors);
    std::vector<double> down_pos_direc;
    down_pos_direc.resize(9);
    if (connector->i_operate == 1 || connector->i_operate == 2 || connector->i_operate == 6 || connector->i_operate == 7){
        down_pos_direc  = connector->kine.kec_joints2End_addh(connector->i_operate);
    }else{
        down_pos_direc  = connector->kine.kec_joints2End();
    }
    std::vector<double> down_pos;
    down_pos.resize(6);
    traj_planning.moveDirection_ver.resize(3);

    //原来按照末端轴线下降方式
    //traj_planning.moveDirection_ver[0] = down_pos_direc[6];
    // traj_planning.moveDirection_ver[1] = down_pos_direc[7];
    // traj_planning.moveDirection_ver[2] = down_pos_direc[8];

    //直接沿世界坐标系y下降
    traj_planning.moveDirection_ver[0] = 0;
    traj_planning.moveDirection_ver[1] = 1;
    traj_planning.moveDirection_ver[2] = 0;
    
    
    for (int i=0; i<6; i++){
        down_pos[i] = down_pos_direc[i];
    }

    double state_length = 0;
    double state_length_total;//通过此参数调整循环次数，进而调整运动距离
    double state_length_retry = 5;
    int state_target = 0;

    if ((connector->i_operate == 0 ||connector->i_operate == 5)||(connector->i_operate == 2 && connector->i_screw == 1))
        {
            state_length_total = 350;//通过此参数调整循环次数，进而调整运动距离
        }
        else if (connector->i_operate == 1||connector->i_operate == 6)
        {
            state_length_total = 550;//通过此参数调整循环次数，进而调整运动距离
        }else if((connector->i_operate == 2 &&connector->i_screw == 0))
        {
            state_length_total = 650;//11.04新加B3
        }
        else if((connector->i_operate == 8 &&connector->i_screw == 0)||(connector->i_operate == 3 &&connector->i_screw == 0)||(connector->i_operate == 4 &&connector->i_screw == 0))
        {
            state_length_total = 200;//通过此参数调整循环次数，进而调整运动距离
        }
        else if((connector->i_operate == 3||connector->i_operate == 2) &&connector->i_screw == 2)
        {
            state_length_total = 500;//通过此参数调整循环次数，进而调整运动距离
        }
         else if((connector->i_operate == 3&&(connector->i_screw == 0||connector->i_screw == 1)))
        {
            state_length_total = 500;//D1,D2
        }
        else
        {
            state_length_total = 400;//通过此参数调整循环次数，进而调整运动距离
        }

    traj_planning.state_hold = 0;
    while(traj_planning.state_hold < state_length_retry)
    //while(state_length < state_length_total)
    {
         state_length = state_length + 1;
        if(state_length > state_length_total){       
            state_target = 1;
        }
        //可以在这里加判断函数

        if (connector->i_operate == 0 ||connector->i_operate == 5)
        {
            down_pos = traj_planning.catesianSpacePlanOnLine(down_pos,state_target,state_length,state_length_total);//竖直下降
        }
        else if(connector->i_operate == 1 ||connector->i_operate == 6||(connector->i_operate == 2 && connector->i_screw == 0)||(connector->i_operate == 7 &&connector->i_screw == 0))
        {
            down_pos = traj_planning.catesianSpacePlanOnLine1(down_pos,state_target,state_length,state_length_total);
        }
        else if((connector->i_operate == 2 &&connector->i_screw == 1)||(connector->i_operate == 7 &&connector->i_screw == 1)||connector->i_operate == 4 && (connector->i_screw == 2)||(connector->i_operate == 9 &&(connector->i_screw == 0 ||connector->i_screw == 1)))//C盖板1 3 4号螺钉
        {
            down_pos = traj_planning.catesianSpacePlanOnLine3(down_pos,state_target,state_length,state_length_total);
        }
        else if((connector->i_operate == 2 &&connector->i_screw == 2)||(connector->i_operate == 7 &&connector->i_screw == 2))//C盖板 2 号螺钉
        {
            down_pos = traj_planning.catesianSpacePlanOnLine4(down_pos,state_target,state_length,state_length_total);
        }
        else if((connector->i_operate == 3 && connector->i_screw == 1)||(connector->i_operate == 4 && connector->i_screw == 1))//D盖板2 4号螺钉
        {
            down_pos = traj_planning.catesianSpacePlanOnLine5(down_pos,state_target,state_length,state_length_total);
        }
        else if((connector->i_operate == 4 && connector->i_screw == 3))//D盖板2 4号螺钉
        {
            down_pos = traj_planning.catesianSpacePlanOnLine6(down_pos,state_target,state_length,state_length_total);
        }
        else
        {
            down_pos = traj_planning.catesianSpacePlanOnLine2(down_pos,state_target,state_length,state_length_total);
        }
   
        std::cout<<"位置："<<down_pos[0]<<" "<<down_pos[1]<<" "<<down_pos[2]<<" "<<"姿态："<<down_pos[3]<<" "<<down_pos[4]<<" "<<down_pos[5]<<"升降舵机: "<< connector->motors[10]<<std::endl;

        connector->kine.Param_writeto_Inv_sol(down_pos);
        // if (connector->i_operate == 1 || connector->i_operate == 2 || connector->i_operate == 6 || connector->i_operate == 7){
        //     std::vector<double>  kinetemp=connector->kine.Inv_sol_addh(connector->i_operate);

        //     connector->motors[0]=kinetemp[0];
        //     connector->motors[1]=kinetemp[1];
        //     connector->motors[2]=kinetemp[2];
        //     connector->motors[6]=kinetemp[6];
        //     connector->motors[7]=kinetemp[7];
        //     connector->motors[8]=kinetemp[8];
        //     connector->motors[9]=kinetemp[9];
        //     connector->motors[10]=kinetemp[10];
        //     // connector->motors=connector->kine.Inv_sol_addh(connector->i_operate);
        // }else{
            
            std::vector<double>  kinetemp=connector->kine.Inv_sol(connector->i_operate, connector->i_screw );

            connector->motors[0]=kinetemp[0];
            connector->motors[1]=kinetemp[1];
            connector->motors[2]=kinetemp[2];
            connector->motors[6]=kinetemp[6];
            connector->motors[7]=kinetemp[7];
            connector->motors[8]=kinetemp[8];
            connector->motors[9]=kinetemp[9];
            connector->motors[10]=kinetemp[10];

            connector->kine.Param_joints_update_simulation_for(kinetemp);

            std::cout<<"下降关节角度"<<kinetemp[0]<<" "<<kinetemp[1]<<" "<<kinetemp[2]<<" "<<kinetemp[3]<<" "<<kinetemp[4]<<" "<<kinetemp[5]<<" "<<kinetemp[10]<<endl;
            //connector->motors=connector->kine.Inv_sol(connector->i_operate, connector->i_screw );
        // }

        //connector->motors[10]=connector->motors[10]+0.1;

        connector->pub_all_robot(connector->motors);
		// connector->pub_total_position(connector->motors,connector->i_operate); //仿真中控制关节速度
    }
    loop->quit();
    return true;
}

//末端轴线上升子函数
bool motionControl_Up(QEventLoop* loop)
{
    connector->kine.Param_joints_update_simulation(connector->motors);
    std::vector<double> up_pos_direc;
    up_pos_direc.resize(9);
    if (connector->i_operate == 1 || connector->i_operate == 2 || connector->i_operate == 6 || connector->i_operate == 7){
        up_pos_direc  = connector->kine.kec_joints2End_addh(connector->i_operate);
    }else{
        up_pos_direc  = connector->kine.kec_joints2End();
    }
    std::vector<double> up_pos;
    up_pos.resize(6);

    //原来按照末端轴线上升方式
    // traj_planning.moveDirection_ver[0] = -traj_planning.moveDirection_ver[0];
    // traj_planning.moveDirection_ver[1] = -traj_planning.moveDirection_ver[1];
    // traj_planning.moveDirection_ver[2] = -traj_planning.moveDirection_ver[2];

    //直接沿世界坐标系y上升
    traj_planning.moveDirection_ver[0] = 0;
    traj_planning.moveDirection_ver[1] = -1;
    traj_planning.moveDirection_ver[2] = 0;

    for (int i=0; i<6; i++){
        up_pos[i] = up_pos_direc[i];
    }

    double state_length = 0;
    double state_length_total;
    double state_length_retry = 5;
    int state_target = 0;
        if (connector->i_operate == 0 ||connector->i_operate == 5)
        {
           state_length_total = 500;//通过此参数调整循环次数，进而调整运动距离
        }
        else if ((connector->i_operate == 2 &&connector->i_screw == 0))
        {
            state_length_total = 600;//通过此参数调整循环次数，进而调整运动距离
        }
        else if(((connector->i_operate == 6) && connector->i_screw == 1)||(connector->i_operate == 8 &&connector->i_screw == 2)||(connector->i_operate == 3 &&connector->i_screw == 0)||(connector->i_operate == 4 &&connector->i_screw == 0)||((connector->i_operate == 2 || connector->i_operate == 7) && (connector->i_screw == 1 )))
        {
            state_length_total = 300;//通过此参数调整循环次数，进而调整运动距离
        }else if((connector->i_operate == 1 &&connector->i_screw == 1)||(connector->i_operate == 3 &&connector->i_screw == 2)||(connector->i_operate == 2 &&connector->i_screw == 2))
        {
            state_length_total = 520;//11.04新加B2
        }else if ((connector->i_operate == 3 &&connector->i_screw == 1))
        {
            state_length_total = 520;//D2
        }
    else
        {
           state_length_total = 420;//通过此参数调整循环次数，进而调整运动距离
        }

    traj_planning.state_hold = 0;
    while(traj_planning.state_hold < state_length_retry)
    //while(state_length < state_length_total)
     {  //bulk_msg_port3.value1
        state_length = state_length + 1;
        if(state_length > state_length_total){     
            state_target = 1;
        }
          if (connector->i_operate == 0 ||connector->i_operate == 5)
        {
            up_pos = traj_planning.catesianSpacePlanOnLine(up_pos,state_target,state_length,state_length_total);//竖直下降

        }
        else if(connector->i_operate == 1 ||connector->i_operate == 6||(connector->i_operate == 2 &&connector->i_screw == 0)||(connector->i_operate == 7 && connector->i_screw == 0))
        {
            up_pos = traj_planning.catesianSpacePlanOnLine1(up_pos,state_target,state_length,state_length_total);
        }
        else if((connector->i_operate == 2 &&connector->i_screw == 1)||(connector->i_operate == 7 &&connector->i_screw == 1)||(connector->i_operate == 4 && (connector->i_screw == 2 ||connector->i_screw == 3))||(connector->i_operate == 9 &&(connector->i_screw == 0 ||connector->i_screw == 1)))//C盖板1 3 4号螺钉
        {
            up_pos = traj_planning.catesianSpacePlanOnLine3(up_pos,state_target,state_length,state_length_total);
        }
        else if((connector->i_operate == 2 &&connector->i_screw == 2)||(connector->i_operate == 7 &&connector->i_screw == 2))//C盖板2 号螺钉
        {
            up_pos = traj_planning.catesianSpacePlanOnLine4(up_pos,state_target,state_length,state_length_total);
        }
        else if((connector->i_operate == 3 &&connector->i_screw == 1)||(connector->i_operate == 4 &&connector->i_screw == 1))//D盖板2 3 4号螺钉
        {
            up_pos = traj_planning.catesianSpacePlanOnLine5(up_pos,state_target,state_length,state_length_total);
        }
        else
        {
            up_pos = traj_planning.catesianSpacePlanOnLine2(up_pos,state_target,state_length,state_length_total);

        }


        std::cout<<"位置："<<up_pos[0]<<" "<<up_pos[1]<<" "<<up_pos[2]<<" "<<"姿态："<<up_pos[3]<<" "<<up_pos[4]<<" "<<up_pos[5]<<"升降舵机: "<< connector->motors[10]<<std::endl;

        connector->kine.Param_writeto_Inv_sol(up_pos);
        // if (connector->i_operate == 1 || connector->i_operate == 2 || connector->i_operate == 6 || connector->i_operate == 7){
        //      std::vector<double>  kinetemp=connector->kine.Inv_sol_addh(connector->i_operate);

        //     connector->motors[0]=kinetemp[0];
        //     connector->motors[1]=kinetemp[1];
        //     connector->motors[2]=kinetemp[2];
        //     connector->motors[6]=kinetemp[6];
        //     connector->motors[7]=kinetemp[7];
        //     connector->motors[8]=kinetemp[8];
        //     connector->motors[9]=kinetemp[9];
        //     connector->motors[10]=kinetemp[10];

        //     //connector->motors=connector->kine.Inv_sol_addh(connector->i_operate);
        // }else{

            std::vector<double>  kinetemp=connector->kine.Inv_sol(connector->i_operate, connector->i_screw );

            connector->motors[0]=kinetemp[0];
            connector->motors[1]=kinetemp[1];
            connector->motors[2]=kinetemp[2];
            connector->motors[6]=kinetemp[6];
            connector->motors[7]=kinetemp[7];
            connector->motors[8]=kinetemp[8];
            connector->motors[9]=kinetemp[9];
            connector->motors[10]=kinetemp[10];

            connector->kine.Param_joints_update_simulation_for(kinetemp);
            //connector->motors=connector->kine.Inv_sol(connector->i_operate, connector->i_screw );
        // }

        //connector->motors[10]=connector->motors[10]-0.1;

        connector->pub_all_robot(connector->motors);
    //    connector->pub_total_position(connector->motors,connector->i_operate); //仿真中控制关节速度
    }
    loop->quit();
    return true;
}

//仿真视频更新子函数
void camera_simu(QEventLoop* loop, Widget* w)
{
    ros::Rate rate_view(50);  //规定前端与检测频率，注释后为尽可能快地刷新
    while(true){
        connector->simulation_image_publish();
        ros::spinOnce();
        rate_view.sleep();
    }
}

//弹窗按钮信号监测       YES::返回7    NO::返回13   CONTINUE::返回0
int window_choose_spin(QEventLoop* loop, int choice_num)
{   
    ros::Rate rate_window(50);
    connector->button_check_continue = 0;
    connector->button_check_Yes_No = 0;
    connector->button_check_left = 0;
    connector->button_check_right = 0;
    connector->button_check_cot = 0;
    connector->button_check_long_push = 0;
    connector->off_touch = false;
    while(!((connector->button_check_Yes_No == 1 && choice_num == 2) || (connector->button_check_continue == 1 && choice_num == 1) || ((connector->button_check_continue == 1 || connector->button_check_continue == -1) && choice_num == 3) || (connector->button_check_continue == 2 && choice_num == 4))){
        ros::spinOnce();
        rate_window.sleep();
    }
    loop->quit();
    return connector->button_check_continue;
}

//长时间不操作的情况下关机(1800s无操作，自动关机)
void deinput_sleep(QEventLoop* loop_clock, Widget* w)
{
    while (w->sleep_clock < 1800)
    {
        if (w->sleep_clock == 0){
            std::cout<<"计时器刷新"<<endl;
        }
        w->sleep_clock++;
        QThread::msleep(1000);
        std::cout<<"关机计时: "<<w->sleep_clock<<"s"<<endl;
    }
    system("sudo shutdown -h now");
}

void screwRemoval(QEventLoop* loop)
{
    QThread::msleep(15000);
    loop->quit();
}

void timeTipUI(QEventLoop* loop)
{
    QThread::msleep(5000);
    loop->quit();
}

void process(Widget* w)
{
    QEventLoop loop;            //线程：运动流程控制
    QEventLoop loop_simu;       //线程：仿真相机与Apriltag检测
    // QEventloop loop_clock;   //线程：计时关机

    //初始化界面（三）开启二维码标记显示
    w->switchPage(U3);
    
    int coverPlate = 0;
    bool normal = false;
    bool runout_tag = false;
    bool loop_break = false;
    int screw_for_A = 0;
    bool screw_1_repair = false;

    //文字提示的依次序字典
    QVector <QString> operate_space = {"P1","P2","P4","P3","P5","Q1","Q2","Q4","Q3","Q5"};
    QVector <QString> coverPlate_space = {"A","B","C","D","E","F","A"};
    QVector <QString> visit_space = {"V1","P1","V2","V2","V3","Q1","V4"};
    QVector <QString> screw_space = {"1","2","3","4"};

    {
        //定时关机开始计时
        // QtConcurrent::run(&deinput_sleep, &loop_clock, w);
    }

    {
        //仿真虚拟摄像头开启
        QtConcurrent::run(&camera_simu, &loop_simu, w);
    }

    for (coverPlate = 0; coverPlate < connector->Index_library.size(); coverPlate++) {

        // 在屏幕中间显示盖板切换信息
        if(coverPlate != 0){
            QString str_1 = QString("<font color = white>%1</font>").arg(" ");
            QString str_2 = QString("<font color = white>%1</font>").arg(" ");
            QString str = QString("<font color = red>%1</font> <font color = red>%2</font> <font color = red>%3</font>")
                    .arg(coverPlate_space[coverPlate - 1], "→", coverPlate_space[coverPlate]);

            w->UIsetText(str_1,str_2);
            w->UIsetText_centre(str);
            // 在屏幕中间显示盖板切换信息
            QtConcurrent::run(&timeTipUI, &loop);
            loop.exec();
            w->UIsetText_centre(QString(" "));
        }  

        if(coverPlate == 0){
            QString str_1 = QString("<font color = white>%1</font>").arg("请确认正确安装工具底座");
            QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font>")
                                .arg("确认后按","OK","继续");
            w->UIsetText(str_1,str_2);
            QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 1);
            loop.exec();
        }else if(coverPlate == 4){
            QString str_1 = QString("<font color = white>%1</font>").arg("请将工具底座安装至对侧扶手");
            QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font>")
                                .arg("装好后按","OK","继续");
            w->UIsetText(str_1,str_2);
            QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 1);
            loop.exec();
        }
        

        if(!screw_1_repair && (coverPlate == 6)){continue;}
        
        connector->i_coverPlate = coverPlate;

        if(coverPlate == 1 || coverPlate == 5){

            {   
                QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = yellow>%4</font> <font color = white>%5</font>")
                                .arg("将","借助工具","观察",coverPlate_space[coverPlate],"锁紧座");
                QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font>")
                                .arg("按","OK","自动调整工具");
                w->UIsetText(str_1,str_2);
            }
            {
                QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 1);
                loop.exec();
            }

            {   
                QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = white>%4</font>")
                                .arg("请与操作环境保持","1m","距离,","手持工具");
                QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = white>%4</font>")
                                .arg("按","OK",",工具运动至","预姿态");
                w->UIsetText(str_1,str_2);
            }
            {
                //提示作用的摇杆信号监测
                QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 1);
                loop.exec();  
            }
            {
                QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = yellow>%4</font>")
                                .arg("工具正在","运动","至","预姿态");
                QString str_2 = QString("<font color = white>%1</font>")
                                .arg("请等待...(约20秒)");
                w->UIsetText(str_1,str_2);
            }
            {
                //探照的关节规划
                QtConcurrent::run(&trajectoryJointPlan_notic, &loop, false, false);
                loop.exec();
            }

            bool ask_break = true;
            while(ask_break){
                if(coverPlate == 1){
                    QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> <font color = yellow>%4</font> <font color = white>%5</font>")
                                    .arg("请将工具右基准面对齐","导轨",visit_space[coverPlate],"标线",",并固定");
                    QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = white>%4</font>")
                                    .arg("固定后按","OK","继续操作","长按结束任务");
                    w->UIsetText(str_1,str_2);
                }else{
                    QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> <font color = yellow>%4</font> <font color = white>%5</font>")
                                    .arg("请将工具左基准面对齐","导轨",visit_space[coverPlate],"标线",",并固定");
                    QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = white>%4</font>")
                                    .arg("固定后按","OK","继续操作","长按结束任务");
                    w->UIsetText(str_1,str_2);
                }
                //提示作用的摇杆信号监测
                QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 3);
                loop.exec();  
                if(result == -1){
                    {
                        QString str_1 = QString("<font color = white>%1</font>")
                                        .arg("确认结束任务并复原工具？");
                        QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                        .arg("取消","(按","N",")","确认","(按","Y",")");
                        w->UIsetText(str_1,str_2);
                    }
                    //提示作用的摇杆信号监测
                    QFuture<int> result_this = QtConcurrent::run(&window_choose_spin, &loop, 2);
                    loop.exec();
                    if(result_this == 7){
                        loop_break = true;
                        ask_break = false;
                        break;
                    }
                }else{
                    ask_break = false;
                }
            }
            if(loop_break == true){break;}
        }else if(screw_1_repair && (coverPlate == 6)){
            normal = false;
        }

        if(coverPlate != 6){
            w->switchPage(U3);
            bool double_check = true;
            while (double_check)
            {
                if(coverPlate == 1 || coverPlate == 5){
                    {   
                        QString str_1 = QString("<font color = white>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> <font color = white>%5</font>")
                                        .arg("借助工具屏幕查看",coverPlate_space[coverPlate],"锁紧座","是否炸断","?");
                        QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                        .arg("未炸断","(按","N",")","炸断","(按","Y",")");
                        w->UIsetText(str_1,str_2);
                    }
                }else{
                    {   
                        QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = yellow>%4</font> <font color = white>%5</font>")
                                        .arg("目视查看",coverPlate_space[coverPlate],"锁紧座","是否炸断","?");
                        QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                        .arg("未炸断","(按","N",")","炸断","(按","Y",")");
                        w->UIsetText(str_1,str_2);
                    }
                }
                //判断是否异常的摇杆信号监测
                QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 2);
                loop.exec();
                normal = (result == 7);
                if(result == 7){
                    {   
                        QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = yellow>%4</font> <font color = white>%5</font>")
                                        .arg("是否确认",coverPlate_space[coverPlate],"锁紧座","炸断","?");
                        QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                        .arg("重新判断","(按","N",")","确认炸断","(按","Y",")");
                        w->UIsetText(str_1,str_2);
                    }
                }else{
                    {   
                        QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = yellow>%4</font> <font color = white>%5</font>")
                                        .arg("是否确认",coverPlate_space[coverPlate],"锁紧座","未炸断","?");
                        QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                        .arg("重新判断","(按","N",")","确认未炸断","(按","Y",")");
                        w->UIsetText(str_1,str_2);
                    }
                }
                {
                    //判断是否异常的摇杆信号监测
                    QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 2);
                    loop.exec(); 
                    if(result == 7){double_check = false;}
                }
            }
        }

        //若发生异常，进入处理程序
        if (!normal) {

            if (coverPlate == 0) {screw_1_repair = true;}

            for (int screw = 0; screw < connector->Index_library[coverPlate].size(); screw++) {
                w->switchPage(U3);
     
                //从盖板->操作位的映射字典中确认当前目标所在操作位
                connector->i_operate = connector->Index_library[coverPlate][screw][0];
                connector->i_screw = connector->Index_library[coverPlate][screw][1];

                //判定是否需要搬出环境
                if((connector->i_operate != connector->operate_load) || (connector->i_operate == 7 && connector->i_screw == 0)){
                    runout_tag = true;
                }
                connector->operate_load = connector->i_operate;

                if (runout_tag) {
                    runout_tag = false;
                    {   
                        QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = white>%4</font>")
                                        .arg("请与操作环境保持","1m","距离,","手持工具");
                        QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = white>%4</font>")
                                .arg("按","OK",",工具运动至","预姿态");
                        w->UIsetText(str_1,str_2);
                    }
                    {
                        //摇杆信号监测
                        QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 1);
                        loop.exec();  
                    }
                    {   
                        QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = white>%4</font>")
                                .arg("工具正在","运动","至","预姿态");
                        QString str_2 = QString("<font color = white>%1</font>")
                                .arg("请等待...(约20秒)");
                        w->UIsetText(str_1,str_2);
                    }
                    {
                        //关节规划
                        QtConcurrent::run(&trajectoryJointPlan, &loop);
                        loop.exec();
                    }
                }
                
                {
                    bool ask_break = true;
                    while(ask_break){
                        if(coverPlate < 4){   
                            QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = yellow>%4</font> <font color = yellow>%5</font> <font color = yellow>%6</font> <font color = white>%7</font>")
                                            .arg("请将工具","右","基准面对齐","导轨",operate_space[connector->i_operate],"标线","并固定");
                            QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = white>%4</font>")
                                            .arg("固定后按","OK","继续操作","长按结束任务");
                            w->UIsetText(str_1,str_2);
                        }else{
                            QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = yellow>%4</font> <font color = yellow>%5</font> <font color = yellow>%6</font> <font color = white>%7</font>")
                                            .arg("请将工具","左","基准面对齐","导轨",operate_space[connector->i_operate],"标线","并固定");
                            QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = white>%4</font>")
                                            .arg("固定后按","OK","继续操作","长按结束任务");
                            w->UIsetText(str_1,str_2);
                        }
                        
                        //摇杆信号监测
                        QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 3);
                        loop.exec();  
                        if(result == -1){
                            {
                                QString str_1 = QString("<font color = white>%1</font>")
                                                .arg("确认结束任务并复原工具？");
                                QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                                .arg("取消","(按","N",")","确认","(按","Y",")");
                                w->UIsetText(str_1,str_2);
                            }
                            //提示作用的摇杆信号监测
                            QFuture<int> result_this = QtConcurrent::run(&window_choose_spin, &loop, 2);
                            loop.exec();
                            if(result_this == 7){
                                loop_break = true;
                                ask_break = false;
                                break;
                            }
                        }else{
                            ask_break = false;
                        }
                    }    
                    if(loop_break == true){break;}            
                }

                if(coverPlate == 6){
                    screw_for_A = screw + 2;
                }else{
                    screw_for_A = screw;
                }

                bool double_check;
                bool visual_servo = true;
                while(visual_servo){
                    {   
                        QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> <font color = white>%5</font> <font color = yellow>%6</font> <font color = white>%7</font> <font color = yellow>%8</font> <font color = white>%9</font>")
                                        .arg("正在进行",coverPlate_space[coverPlate],screw_space[screw_for_A],"螺钉粗定位","(",coverPlate_space[coverPlate],"锁紧座共","4","颗)");
                        QString str_2 = QString("<font color = white>%1</font>")
                                        .arg("请等待...(约20秒)");
                        w->UIsetText(str_1,str_2);
                    }
                    {
                        //笛卡尔轨迹规划
                        QtConcurrent::run(&screwPosControl, &loop);
                        loop.exec();
                    }

                    bool capture_QR = false;
                    while(!capture_QR){
                        while(connector->apriltag_position_posture[6] == 0 /*判断未捕捉到相应螺钉的二维码*/){
                            {   
                                QString str_1 = QString("<font color = yellow>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%4</font>")
                                                .arg(coverPlate_space[coverPlate],screw_space[screw_for_A],"粗定位结束","未在识别范围内");
                                QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = yellow>%4</font>")
                                                .arg("按","OK","进行","手动调整");
                                w->UIsetText(str_1,str_2);
                            }
                            {
                                //摇杆信号监测
                                QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 1);
                                loop.exec();  
                            }
                            {   
                                QString str_1 = QString("<font color = yellow>%1</font> <font color = white>%2</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = white>%3</font>")
                                                .arg(" "," ","处于安全范围");
                                QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font>")
                                                .arg("出现二维码后&nbsp;按","OK","精定位");
                                w->UIsetText(str_1,str_2);
                            }
                            {
                                QtConcurrent::run(&manualModeExample, &loop);
                                loop.exec();
                            }
                        }
                        {
                            {   
                                QString str_1 = QString("<font color = yellow>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%4</font>")
                                                .arg(coverPlate_space[coverPlate],screw_space[screw_for_A],"粗定位完成","已捕获二维码");
                                QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font>")
                                                .arg("按","OK","自动调整");
                                w->UIsetText(str_1,str_2);
                            }
                            {
                                //摇杆信号监测
                                QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 1);
                                loop.exec();  
                                capture_QR = true;
                            }
                        }
                    }

                    {   
                        QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> <font color = white>%5</font> <font color = yellow>%6</font> <font color = white>%7</font> <font color = yellow>%8</font> <font color = white>%9</font>")
                                        .arg("正在进行",coverPlate_space[coverPlate],screw_space[screw_for_A],"螺钉精定位","(",coverPlate_space[coverPlate],"锁紧座共","4","颗)");
                        QString str_2 = QString("<font color = white>%1</font>")
                                        .arg("请等待...(约20秒)");
                        w->UIsetText(str_1,str_2);
                    }
                    {
                        QFuture<bool> result = QtConcurrent::run(&screwVisualServo, &loop);
                        loop.exec();
                        visual_servo = !result;
                    }
                    if(visual_servo){   
                        {
                            QString str_1 = QString("<font color = yellow>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = white>%4</font>")
                                            .arg(coverPlate_space[coverPlate],screw_space[screw_for_A],"精定位失败","请重试");
                            QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                            .arg("手动重试","(按","N",")","自动重试","(按","Y",")");
                            w->UIsetText(str_1,str_2);
                        }
                        //摇杆信号监测
                        QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 2);
                        loop.exec(); 
                        if(result != 7){
                            //进入手动
                            double_check = true;
                            visual_servo = false;
                        }
                    }else{   
                        w->switchPage(U2);
                        QString str_1 = QString("<font color = yellow>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = white>%4</font>")
                                        .arg(coverPlate_space[coverPlate],screw_space[screw_for_A],"精定位已完成","将套入螺钉");
                        QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                        .arg("手动套入","(按","N",")","自动套入","(按","Y",")");
                        w->UIsetText(str_1,str_2);
                        //摇杆信号监测
                        QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 2);
                        loop.exec(); 
                        if(result == 7){
                            double_check = false;
                        }else{
                            double_check = true;
                        }
                    }
                }
                
                w->switchPage(U1);
                while(double_check)
                {
                    {   
                        QString str_1 = QString("<font color = yellow>%1</font> <font color = white>%2</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = white>%3</font>")
                                        .arg(" "," ","处于安全范围");
                        QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font>")
                                        .arg("轮廓对准后&nbsp;按","OK","自动下落");
                        w->UIsetText(str_1,str_2);
                    }
                    QtConcurrent::run(&manualModeExample, &loop);
                    loop.exec();
                    
                    {   
                        QString str_1 = QString("<font color = yellow>%1</font>")
                                        .arg("确认是否对准");
                        QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                        .arg("未对准","(按","N",")","对准","(按","Y",")");
                        w->UIsetText(str_1,str_2);
                    }
                    {
                        //确认对准的摇杆信号监测
                        QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 2);
                        loop.exec();  
                        if (result == 7){
                            double_check = false;
                        }
                    }
                }

                {
                    QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                    .arg("套筒正在套入",coverPlate_space[coverPlate],screw_space[screw_for_A],"螺钉(",coverPlate_space[coverPlate],"锁紧座共","4","颗)");
                    QString str_2 = QString("<font color = white>%1</font>").arg("请等待...(约10秒)");
                    w->UIsetText(str_1,str_2);
                }
                {
                    QtConcurrent::run(&motionControl_Down, &loop);
                    loop.exec();
                }
                w->switchPage(U1);
                /*if(connector->limited_joint == 0)*/
                {
                    //判断是否套上
                    bool catchon_check = true;
                    QFuture<int> cover_on;
                    while(catchon_check){
                        bool double_check = true;
                        while (double_check){
                        
                            ros::spinOnce();
                            if(connector->screw_catch_button){
                                {   
                                    QString str_1 = QString("<font color = white>%1</font> &nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%2</font>")
                                                    .arg("检测到已套上","请判断工具是否套上螺钉?");
                                    QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                                    .arg("未套上","(按","N",")","已套上","(按","Y",")");
                                    w->UIsetText(str_1,str_2);
                                }
                            }else{
                                {   
                                    QString str_1 = QString("<font color = white>%1</font> &nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%2</font>")
                                                    .arg("检测到未套上","请判断工具是否套上螺钉?");
                                    QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                                    .arg("未套上","(按","N",")","已套上","(按","Y",")");
                                    w->UIsetText(str_1,str_2);
                                }
                            }

                            cover_on = QtConcurrent::run(&window_choose_spin, &loop, 2);
                            loop.exec();

                            if(cover_on == 7){
                                {   
                                    QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> <font color = yellow>%5</font> <font color = white>%6</font>")
                                                    .arg("是否确认",coverPlate_space[coverPlate],screw_space[screw_for_A],"螺钉","已套上","?");
                                    QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                                    .arg("重新判断","(按","N",")","确认套上","(按","Y",")");
                                    w->UIsetText(str_1,str_2);
                                }
                            }else{
                                {   
                                    QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> <font color = yellow>%5</font> <font color = white>%6</font>")
                                                    .arg("是否确认",coverPlate_space[coverPlate],screw_space[screw_for_A],"螺钉","未套上","?");
                                    QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                                    .arg("重新判断","(按","N",")","确认未套上","(按","Y",")");
                                    w->UIsetText(str_1,str_2);
                                }
                            }
                            {
                                //判断是否异常的摇杆信号监测
                                QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 2);
                                loop.exec(); 
                                if(result == 7){double_check = false;}
                            }
                        }

                        if(cover_on == 13){
                            {   
                                QString str_1 = QString("<font color = yellow>%1</font> <font color = white>%2</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = white>%3</font>")
                                                .arg(" "," ","处于安全范围");
                                QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font>")
                                                .arg("套上螺钉后&nbsp;按","OK","继续");
                                w->UIsetText(str_1,str_2);
                            }
                            w->switchPage(U2);
                            QtConcurrent::run(&manualModeExample, &loop);
                            loop.exec();
                            w->switchPage(U3);
                        }else{
                            catchon_check = false;
                        }
                    }
                }/*else{
                    {   
                        QString str_1 = QString("<font color = yellow>%1</font> <font color = yellow>%2</font>")
                                        .arg("工具","已接触限位");
                        QString str_2 = QString("<font color = yellow>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = yellow>%4</font> <font color = white>%5</font>")
                                        .arg("尝试调整末端","以套上后","按","K","继续");
                        w->UIsetText(str_1,str_2);
                    }
                    {
                        //摇杆信号监测
                        QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 1);
                        loop.exec();
                    }
                }*/

                {   
                    QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                    .arg("套筒已套上",coverPlate_space[coverPlate],screw_space[screw_for_A],"螺钉(",coverPlate_space[coverPlate],"锁紧座共","4","颗)");
                    QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = white>%4</font> <font color = yellow>%5</font>")
                                    .arg("请按","M","键","进行","螺钉拆卸");
                    w->UIsetText(str_1,str_2);
                }
                {
                    //摇杆信号监测
                    QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 4);
                    loop.exec();
                }
                {   
                    QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                    .arg("工具正在拧松",coverPlate_space[coverPlate],screw_space[screw_for_A],"螺钉(",coverPlate_space[coverPlate],"锁紧座共","4","颗)");
                    QString str_2 = QString("<font color = white>%1</font>")
                                    .arg("请等待...(约15秒)");
                    w->UIsetText(str_1,str_2);
                }
                {
                    //等待10秒
                    QtConcurrent::run(&screwRemoval, &loop);
                    loop.exec();
                }
                {   
                    QString str_1 = QString("<font color = yellow>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = yellow>%4</font> <font color = white>%5</font> <font color = yellow>%6</font> <font color = white>%7</font>")
                                    .arg(coverPlate_space[coverPlate],screw_space[screw_for_A],"螺钉已被拧松(",coverPlate_space[coverPlate],"锁紧座共","4","颗)");
                    QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font>")
                                    .arg("请按","OK","继续操作");
                    w->UIsetText(str_1,str_2);
                }
                {
                    //摇杆信号监测
                    QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 1);
                    loop.exec();
                }
                {
                    QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                    .arg("套筒正在远离",coverPlate_space[coverPlate],screw_space[screw_for_A],"螺钉(",coverPlate_space[coverPlate],"锁紧座共","4","颗)");
                    QString str_2 = QString("<font color = white>%1</font>").arg("请等待...(约10秒)");
                    w->UIsetText(str_1,str_2);
                }
                {
                    QtConcurrent::run(&motionControl_Up, &loop);
                    loop.exec();
                }
                
                //判断是否升出
                bool getoff_check = true;
                QFuture<int> get_off;
                while(getoff_check){
                    bool double_check = true;
                    while(double_check){
                        {   
                            QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> <font color = yellow>%5</font> <font color = white>%6</font>")
                                                .arg("观察套筒是否与",coverPlate_space[coverPlate],screw_space[screw_for_A],"螺钉","分离","?");
                            QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                            .arg("未分离","(按","N",")-手动调整","已分离","(按","Y",")");
                            w->UIsetText(str_1,str_2);
                        }
                        //摇杆信号监测
                        get_off = QtConcurrent::run(&window_choose_spin, &loop, 2);
                        loop.exec();
                    
                        if(get_off == 7){
                            {   
                                QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> <font color = yellow>%5</font> <font color = white>%6</font>")
                                                .arg("是否确认与",coverPlate_space[coverPlate],screw_space[screw_for_A],"螺钉","分离","?");
                                QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                                .arg("重新判断","(按","N",")","确认分离","(按","Y",")");
                                w->UIsetText(str_1,str_2);
                            }
                        }else{
                            {   
                                QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> <font color = yellow>%5</font> <font color = white>%6</font>")
                                                .arg("是否确认与",coverPlate_space[coverPlate],screw_space[screw_for_A],"螺钉","未分离","?");
                                QString str_2 = QString("<font color = yellow>%1</font> <font color = white>%2</font> <font color = yellow>%3</font> <font color = white>%4</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = yellow>%5</font> <font color = white>%6</font> <font color = yellow>%7</font> <font color = white>%8</font>")
                                                .arg("重新判断","(按","N",")","确认未分离","(按","Y",")");
                                w->UIsetText(str_1,str_2);
                            }
                        }
                        {
                            //判断是否异常的摇杆信号监测
                            QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 2);
                            loop.exec(); 
                            if(result == 7){double_check = false;}
                        }
                    }

                    if(get_off == 13){  //没升出进入到手动调整
                        {   
                            QString str_1 = QString("<font color = yellow>%1</font> <font color = white>%2</font> &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; <font color = white>%3</font>")
                                            .arg(" "," ","处于安全范围");
                            QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font>")
                                            .arg("离开螺钉后&nbsp;按","OK","继续");
                            w->UIsetText(str_1,str_2);
                        }
                        QtConcurrent::run(&manualModeExample, &loop);
                        loop.exec();
                    }else{
                        getoff_check = false;
                    }
                }
            }
            if(loop_break){
                break;
            }
        }
        
        //某些盖板探视后需要搬出环境
        else {
            if (coverPlate == 1 || coverPlate == 5) {
                {   
                    QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = yellow>%4</font> <font color = white>%5</font>")
                                    .arg("请将工具","移至舱外",",与环境保持","1m","距离");
                    QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font>  <font color = yellow>%4</font> <font color = white>%5</font>")
                                    .arg("按","OK","继续");
                    w->UIsetText(str_1,str_2);
                }
                {
                    //摇杆信号监测
                    QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 1);
                    loop.exec();  
                }
            }
        }
    }
    
    //结束检查工作后恢复原状
    {   
        QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> <font color = yellow>%4</font>")
                        .arg("检修已","完成",",请手持工具","保证足够运动空间");
        QString str_2 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font> ")
                        .arg("按","OK","进行工具复位");
        w->UIsetText(str_1,str_2);
    }
    {
        //摇杆信号监测
        QFuture<int> result = QtConcurrent::run(&window_choose_spin, &loop, 1);
        loop.exec();  
    }
    {   
        QString str_1 = QString("<font color = white>%1</font> <font color = yellow>%2</font> <font color = white>%3</font>")
                        .arg("正在进行","工具复位",",请等待...");
        QString str_2 = QString("<font color = yellow>%1</font>")
                        .arg("复位完成后将自动关机");
        w->UIsetText(str_1,str_2);
    }
    {
        QtConcurrent::run(&trajectoryJointPlan_notic, &loop, false, true);
        loop.exec();
    }
}


int main(int argc, char *argv[])
{
    ros::init(argc, argv, "rosconnector");
    ros::NodeHandle nh;

    QApplication a(argc, argv);
    Widget w;
    w.setWindowFlags(Qt::FramelessWindowHint);
    w.show();
    connector = new RosConnector(nh, w);

    {
        QShortcut *temp = new QShortcut(&w);
        temp->setKey(QObject::tr("1"));
        temp->setAutoRepeat(false);
        QObject::connect(temp, &QShortcut::activated, &w, [&w]() {
            // w.switchPage(U1);
        });
    }
    {
        QShortcut *temp = new QShortcut(&w);
        temp->setKey(QObject::tr("2"));
        temp->setAutoRepeat(false);
        QObject::connect(temp, &QShortcut::activated, &w, [&w]() {
            // w.switchPage(U2);
        });
    }
    {
        QShortcut *temp = new QShortcut(&w);
        temp->setKey(QObject::tr("3"));
        temp->setAutoRepeat(false);
        QObject::connect(temp, &QShortcut::activated, &w, [&w]() {
            // w.switchPage(U3);
        });
    }

    process(&w);
    delete connector;
    system("sudo shutdown -h now");

    return a.exec();
}
